
#ifndef MATRIX4_H_
#define MATRIX4_H_

#include "Vector4.h"

namespace p2 {

// Rotation matrices are of the form
//   R = cos(t) -sin(t)
//       sin(t)  cos(t)
// where t > 0 indicates a counterclockwise rotation in the xy-plane.

template<typename Real>
class Matrix4
{
public:
	inline Matrix4();	// uninitialised
	inline Matrix4(const Matrix4& mat);
	inline Matrix4(	Real m00, Real m01, Real m02, Real m03,
						Real m10, Real m11, Real m12, Real m13,
						Real m20, Real m21, Real m22, Real m23,
						Real m30, Real m31, Real m32, Real m33	);
	inline Matrix4(Real const entry[16], bool rowMajor);
	inline Matrix4(Real m00, Real m11, Real m22, Real m33);

	// Assignment.
	inline Matrix4& operator=(Matrix4 const& mat);

	// Arithmetic operations.
	inline Matrix4 operator+(Matrix4 const& mat) const;
	inline Matrix4 operator-(Matrix4 const& mat) const;
	inline Matrix4 operator*(Real scalar) const;
	inline Matrix4 operator/(Real scalar) const;
	inline Matrix4 operator-() const;

	// Arithmetic updates.
	inline Matrix4& operator+=(Matrix4 const& mat);
	inline Matrix4& operator-=(Matrix4 const& mat);
	inline Matrix4& operator*=(Real scalar);
	inline Matrix4& operator/=(Real scalar);

	// access
	inline Real const* operator[](int row) const;
	inline Real* operator[](int row);
	inline operator Real const*() const;
	inline operator Real*();

	// M*vec
	inline Vector4<Real> operator*(Vector4<Real> const& v) const;

	// M^T
	inline Matrix4 transpose() const;

	// M*mat
	inline Matrix4 operator*(Matrix4 const& mat) const;

	// create matrices
	inline static Matrix4 makeZero();
	inline static Matrix4 makeIdentity();
	inline static Matrix4 makeTranslation(Real tx, Real ty, Real tz);
	inline static Matrix4 makeScale(Real sx, Real sy, Real sz);
	inline static Matrix4 makeDiagonal(Real m00, Real m11, Real m22, Real m33);
	inline static Matrix4 makeOrtho(	Real left, Real right,
										Real bottom, Real top,
										Real nearZ, Real farZ	);

	/*
	 * pre-defined
	 */
	static const Matrix4 ZERO;
	static const Matrix4 IDENTITY;

protected:
	Real m_t[16];
};

// c * M
template<typename Real>
inline Matrix4<Real> operator*(Real scalar, Matrix4<Real> const& mat);

// v^T * M
template<typename Real>
inline Vector4<Real> operator*(Vector4<Real> const& vec, Matrix4<Real> const& mat);

template<typename Real>
inline Matrix4<Real>::Matrix4()
{
}

template<typename Real>
inline Matrix4<Real>::Matrix4(Matrix4 const& mat)
{
	m_t[0] = mat.m_t[0];	m_t[1] = mat.m_t[1];	m_t[2] = mat.m_t[2];	m_t[3] = mat.m_t[3];
	m_t[4] = mat.m_t[4];	m_t[5] = mat.m_t[5];	m_t[6] = mat.m_t[6];	m_t[7] = mat.m_t[7];
	m_t[8] = mat.m_t[8];	m_t[9] = mat.m_t[9];	m_t[10] = mat.m_t[10];	m_t[11] = mat.m_t[11];
	m_t[12] = mat.m_t[12];	m_t[13] = mat.m_t[13];	m_t[14] = mat.m_t[14];	m_t[15] = mat.m_t[15];
}

template<typename Real>
inline Matrix4<Real>::Matrix4(	Real m00, Real m01, Real m02, Real m03,
								Real m10, Real m11, Real m12, Real m13,
								Real m20, Real m21, Real m22, Real m23,
								Real m30, Real m31, Real m32, Real m33	)
{
	m_t[0] = m00;	m_t[1] = m01;	m_t[2] = m02;	m_t[3] = m03;
	m_t[4] = m10;	m_t[5] = m11;	m_t[6] = m12;	m_t[7] = m13;
	m_t[8] = m20;	m_t[9] = m21;	m_t[10] = m22;	m_t[11] = m23;
	m_t[12] = m30;	m_t[13] = m31;	m_t[14] = m32;	m_t[15] = m33;
}

template<typename Real>
inline Matrix4<Real>::Matrix4(Real const e[16], bool rowMajor)
{
	if (rowMajor)
	{
		m_t[0] = e[0];		m_t[1] = e[1];		m_t[2] = e[2];		m_t[3] = e[3];
		m_t[4] = e[4];		m_t[5] = e[5];		m_t[6] = e[6];		m_t[7] = e[7];
		m_t[8] = e[8];		m_t[9] = e[9];		m_t[10] = e[10];	m_t[11] = e[11];
		m_t[12] = e[12];	m_t[13] = e[13];	m_t[14] = e[14];	m_t[15] = e[15];
	}
	else
	{
		m_t[0] = e[0];		m_t[4] = e[1];		m_t[8] = e[2];		m_t[12] = e[3];
		m_t[1] = e[4];		m_t[5] = e[5];		m_t[9] = e[6];		m_t[13] = e[7];
		m_t[2] = e[8];		m_t[6] = e[9];		m_t[10] = e[10];	m_t[14] = e[11];
		m_t[3] = e[12];		m_t[7] = e[13];		m_t[11] = e[14];	m_t[15] = e[15];
	}
}

template<typename Real>
inline Matrix4<Real>::Matrix4(Real m00, Real m11, Real m22, Real m33)
{
	makeDiagonal(m00, m11, m22, m33);
}

template<typename Real>
inline Matrix4<Real>& Matrix4<Real>::operator=(Matrix4 const& mat)
{
	if (this != &mat)
	{
		m_t[0] = mat.m_t[0];	m_t[1] = mat.m_t[1];	m_t[2] = mat.m_t[2];	m_t[3] = mat.m_t[3];
		m_t[4] = mat.m_t[4];	m_t[5] = mat.m_t[5];	m_t[6] = mat.m_t[6];	m_t[7] = mat.m_t[7];
		m_t[8] = mat.m_t[8];	m_t[9] = mat.m_t[9];	m_t[10] = mat.m_t[10];	m_t[11] = mat.m_t[11];
		m_t[12] = mat.m_t[12];	m_t[13] = mat.m_t[13];	m_t[14] = mat.m_t[14];	m_t[15] = mat.m_t[15];
	}
	return *this;
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::operator+(Matrix4 const& mat) const
{
	return Matrix4<Real>(
		m_t[0] + mat.m_t[0],	m_t[1] + mat.m_t[1],	m_t[2] + mat.m_t[2],	m_t[3] + mat.m_t[3],
		m_t[4] + mat.m_t[4],	m_t[5] + mat.m_t[5],	m_t[6] + mat.m_t[6],	m_t[7] + mat.m_t[7],
		m_t[8] + mat.m_t[8],	m_t[9] + mat.m_t[9],	m_t[10] + mat.m_t[10],	m_t[11] + mat.m_t[11],
		m_t[12] + mat.m_t[12],	m_t[13] + mat.m_t[13],	m_t[14] + mat.m_t[14],	m_t[15] + mat.m_t[15]
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::operator-(Matrix4 const& mat) const
{
	return Matrix4<Real>(
		m_t[0] - mat.m_t[0],	m_t[1] - mat.m_t[1],	m_t[2] - mat.m_t[2],	m_t[3] - mat.m_t[3],
		m_t[4] - mat.m_t[4],	m_t[5] - mat.m_t[5],	m_t[6] - mat.m_t[6],	m_t[7] - mat.m_t[7],
		m_t[8] - mat.m_t[8],	m_t[9] - mat.m_t[9],	m_t[10] - mat.m_t[10],	m_t[11] - mat.m_t[11],
		m_t[12] - mat.m_t[12],	m_t[13] - mat.m_t[13],	m_t[14] - mat.m_t[14],	m_t[15] - mat.m_t[15]
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::operator*(Real scalar) const
{
	return Matrix4<Real>(
		scalar * m_t[0], scalar * m_t[1], scalar * m_t[2], scalar * m_t[3],
		scalar * m_t[4], scalar * m_t[5], scalar * m_t[6], scalar * m_t[7],
		scalar * m_t[8], scalar * m_t[9], scalar * m_t[10], scalar * m_t[11],
		scalar * m_t[12], scalar * m_t[13], scalar * m_t[14], scalar * m_t[15]
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::operator/(Real scalar) const
{
	if (scalar != (Real)0) {
		Real invScalar = ((Real)1) / scalar;
		return Matrix4<Real>(
			invScalar * m_t[0], invScalar * m_t[1], invScalar * m_t[2], invScalar * m_t[3],
			invScalar * m_t[4], invScalar * m_t[5], invScalar * m_t[6], invScalar * m_t[7],
			invScalar * m_t[8], invScalar * m_t[9], invScalar * m_t[10], invScalar * m_t[11],
			invScalar * m_t[12], invScalar * m_t[13], invScalar * m_t[14], invScalar * m_t[15]
		);
	} else {
		return Matrix4<Real>(
			Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL,
			Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL,
			Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL,
			Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL
		);
	}
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::operator-() const
{
	return Matrix4<Real>(
		-m_t[0], -m_t[1], -m_t[2], -m_t[3],
		-m_t[4], -m_t[5], -m_t[6], -m_t[7],
		-m_t[8], -m_t[9], -m_t[10], -m_t[11],
		-m_t[12], -m_t[13], -m_t[14], -m_t[15]
	);
}

template<typename Real>
inline Matrix4<Real>& Matrix4<Real>::operator+=(const Matrix4& mat)
{
	m_t[0] += mat.m_t[0];	m_t[1] += mat.m_t[1];	m_t[2] += mat.m_t[2];	m_t[3] += mat.m_t[3];
	m_t[4] += mat.m_t[4];	m_t[5] += mat.m_t[5];	m_t[6] += mat.m_t[6];	m_t[7] += mat.m_t[7];
	m_t[8] += mat.m_t[8];	m_t[9] += mat.m_t[9];	m_t[10] += mat.m_t[10];	m_t[11] += mat.m_t[11];
	m_t[12] += mat.m_t[12];	m_t[13] += mat.m_t[13];	m_t[14] += mat.m_t[14];	m_t[15] += mat.m_t[15];
	return *this;
}

template<typename Real>
inline Matrix4<Real>& Matrix4<Real>::operator-=(Matrix4 const& mat)
{
	m_t[0] -= mat.m_t[0];	m_t[1] -= mat.m_t[1];	m_t[2] -= mat.m_t[2];	m_t[3] -= mat.m_t[3];
	m_t[4] -= mat.m_t[4];	m_t[5] -= mat.m_t[5];	m_t[6] -= mat.m_t[6];	m_t[7] -= mat.m_t[7];
	m_t[8] -= mat.m_t[8];	m_t[9] -= mat.m_t[9];	m_t[10] -= mat.m_t[10];	m_t[11] -= mat.m_t[11];
	m_t[12] -= mat.m_t[12];	m_t[13] -= mat.m_t[13];	m_t[14] -= mat.m_t[14];	m_t[15] -= mat.m_t[15];
	return *this;
}

template<typename Real>
inline Matrix4<Real>& Matrix4<Real>::operator*=(Real scalar)
{
	m_t[0] *= scalar;	m_t[1] *= scalar;	m_t[2] *= scalar;	m_t[3] *= scalar;
	m_t[4] *= scalar;	m_t[5] *= scalar;	m_t[6] *= scalar;	m_t[7] *= scalar;
	m_t[8] *= scalar;	m_t[9] *= scalar;	m_t[10] *= scalar;	m_t[11] *= scalar;
	m_t[12] *= scalar;	m_t[13] *= scalar;	m_t[14] *= scalar;	m_t[15] *= scalar;
	return *this;
}

template<typename Real>
inline Matrix4<Real>& Matrix4<Real>::operator/=(Real scalar)
{
	if (scalar != (Real) 0) {
		Real invScalar = ((Real) 1) / scalar;
		m_t[0] *= invScalar;	m_t[1] *= invScalar;	m_t[2] *= invScalar;	m_t[3] *= invScalar;
		m_t[4] *= invScalar;	m_t[5] *= invScalar;	m_t[6] *= invScalar;	m_t[7] *= invScalar;
		m_t[8] *= invScalar;	m_t[9] *= invScalar;	m_t[10] *= invScalar;	m_t[11] *= invScalar;
		m_t[12] *= invScalar;	m_t[13] *= invScalar;	m_t[14] *= invScalar;	m_t[15] *= invScalar;
	} else {
		m_t[0] = Math<Real>::MAX_REAL;	m_t[1] = Math<Real>::MAX_REAL;	m_t[2] = Math<Real>::MAX_REAL;	m_t[3] = Math<Real>::MAX_REAL;
		m_t[4] = Math<Real>::MAX_REAL;	m_t[5] = Math<Real>::MAX_REAL;	m_t[6] = Math<Real>::MAX_REAL;	m_t[7] = Math<Real>::MAX_REAL;
		m_t[8] = Math<Real>::MAX_REAL;	m_t[9] = Math<Real>::MAX_REAL;	m_t[10] = Math<Real>::MAX_REAL;	m_t[11] = Math<Real>::MAX_REAL;
		m_t[12] = Math<Real>::MAX_REAL;	m_t[13] = Math<Real>::MAX_REAL;	m_t[14] = Math<Real>::MAX_REAL;	m_t[15] = Math<Real>::MAX_REAL;
	}
	return *this;
}

template<typename Real>
inline Vector4<Real> Matrix4<Real>::operator*(Vector4<Real> const& v) const
{
	return Vector4<Real>(
		m_t[0] * v[0] + m_t[1] * v[1] + m_t[2] * v[2] + m_t[3] * v[3],
		m_t[4] * v[0] + m_t[5] * v[1] + m_t[6] * v[2] + m_t[7] * v[3],
		m_t[8] * v[0] + m_t[9] * v[1] + m_t[10] * v[2] + m_t[11] * v[3],
		m_t[12] * v[0] + m_t[13] * v[1] + m_t[14] * v[2] + m_t[15] * v[3]
	);
}
template<typename Real>
inline Vector4<Real> operator*(Vector4<Real> const& v, Matrix4<Real> const& m)
{
	return Vector4<Real>(
		m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2] + m[3][0] * v[3],
		m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2] + m[3][1] * v[3],
		m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2] + m[3][2] * v[3],
		m[0][3] * v[0] + m[1][3] * v[1] + m[2][3] * v[2] + m[3][3] * v[3]
	);
}
template<typename Real>
inline Real const* Matrix4<Real>::operator[](int row) const
{
	return &m_t[4 * row];
}
template<typename Real>
inline Real* Matrix4<Real>::operator[](int row)
{
	return &m_t[4 * row];
}
template<typename Real>
inline Matrix4<Real>::operator Real*()
{
	return &m_t[0];
}

template<typename Real>
inline Matrix4<Real>::operator Real const*() const
{
	return &m_t[0];
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::makeZero()
{
	return Matrix4<Real>(
		(Real)0, (Real)0, (Real)0, (Real)0,
		(Real)0, (Real)0, (Real)0, (Real)0,
		(Real)0, (Real)0, (Real)0, (Real)0,
		(Real)0, (Real)0, (Real)0, (Real)0
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::makeIdentity()
{
	return Matrix4<Real>(
		(Real)1, (Real)0, (Real)0, (Real)0,
		(Real)0, (Real)1, (Real)0, (Real)0,
		(Real)0, (Real)0, (Real)1, (Real)0,
		(Real)0, (Real)0, (Real)0, (Real)1
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::makeTranslation(Real tx, Real ty, Real tz)
{
	return Matrix4<Real>(
		(Real)1, (Real)0, (Real)0, (Real)0,
		(Real)0, (Real)1, (Real)0, (Real)0,
		(Real)0, (Real)0, (Real)1, (Real)0,
		tx,	ty, tz, (Real)1
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::makeScale(Real sx, Real sy, Real sz)
{
	return Matrix4<Real>(
		sx, (Real)0, (Real)0, (Real)0,
		(Real)0, sy, (Real)0, (Real)0,
		(Real)0, (Real)0, sz, (Real)0,
		(Real)0, (Real)0, (Real)0, (Real)1
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::makeDiagonal(Real m00, Real m11, Real m22, Real m33)
{
	return Matrix4<Real>(
		m00, (Real)0, (Real)0, (Real)0,
		(Real)0, m11, (Real)0, (Real)0,
		(Real)0, (Real)0, m22, (Real)0,
		(Real)0, (Real)0, (Real)0, m33
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::makeOrtho(	Real left, Real right,
												Real bottom, Real top,
												Real nearZ, Real farZ	)
{
    Real ral = right + left;
    Real rsl = right - left;
    Real tab = top + bottom;
    Real tsb = top - bottom;
    Real fan = farZ + nearZ;
    Real fsn = farZ - nearZ;
    
	return Matrix4<Real>(
		(Real)2 / rsl, (Real)0, (Real)0, (Real)0,
		(Real)0, (Real)2 / tsb, (Real)0, (Real)0,
		(Real)0, (Real)0, (Real)-2 / fsn, (Real)0,
		-ral / rsl, -tab / tsb, -fan / fsn, (Real)1
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::transpose() const
{
	return Matrix4<Real>(
		m_t[0], m_t[4], m_t[8], m_t[12],
		m_t[1], m_t[5], m_t[9], m_t[13],
		m_t[2], m_t[6], m_t[10], m_t[14],
		m_t[3], m_t[7], m_t[11], m_t[15]
	);
}

template<typename Real>
inline Matrix4<Real> Matrix4<Real>::operator*(Matrix4 const& m) const
{
	// this * m
	return Matrix4<Real>(
		m_t[0] * m.m_t[0]  + m_t[1] * m.m_t[4]  + m_t[2] * m.m_t[8]  + m_t[3] * m.m_t[12],
		m_t[0] * m.m_t[1]  + m_t[1] * m.m_t[5]  + m_t[2] * m.m_t[9]  + m_t[3] * m.m_t[13],
		m_t[0] * m.m_t[2]  + m_t[1] * m.m_t[6]  + m_t[2] * m.m_t[10] + m_t[3] * m.m_t[14],
		m_t[0] * m.m_t[3]  + m_t[1] * m.m_t[7]  + m_t[2] * m.m_t[11] + m_t[3] * m.m_t[15],

		m_t[4] * m.m_t[0]  + m_t[5] * m.m_t[4]  + m_t[6] * m.m_t[8]  + m_t[7] * m.m_t[12],
		m_t[4] * m.m_t[1]  + m_t[5] * m.m_t[5]  + m_t[6] * m.m_t[9]  + m_t[7] * m.m_t[13],
		m_t[4] * m.m_t[2]  + m_t[5] * m.m_t[6]  + m_t[6] * m.m_t[10] + m_t[7] * m.m_t[14],
		m_t[4] * m.m_t[3]  + m_t[5] * m.m_t[7]  + m_t[6] * m.m_t[11] + m_t[7] * m.m_t[15],

		m_t[8] * m.m_t[0]  + m_t[9] * m.m_t[4]  + m_t[10] * m.m_t[8]  + m_t[11] * m.m_t[12],
		m_t[8] * m.m_t[1]  + m_t[9] * m.m_t[5]  + m_t[10] * m.m_t[9]  + m_t[11] * m.m_t[13],
		m_t[8] * m.m_t[2]  + m_t[9] * m.m_t[6]  + m_t[10] * m.m_t[10] + m_t[11] * m.m_t[14],
		m_t[8] * m.m_t[3]  + m_t[9] * m.m_t[7]  + m_t[10] * m.m_t[11] + m_t[11] * m.m_t[15],

		m_t[12] * m.m_t[0] + m_t[13] * m.m_t[4] + m_t[14] * m.m_t[8]  + m_t[15] * m.m_t[12],    
		m_t[12] * m.m_t[1] + m_t[13] * m.m_t[5] + m_t[14] * m.m_t[9]  + m_t[15] * m.m_t[13],
		m_t[12] * m.m_t[2] + m_t[13] * m.m_t[6] + m_t[14] * m.m_t[10] + m_t[15] * m.m_t[14],
		m_t[12] * m.m_t[3] + m_t[13] * m.m_t[7] + m_t[14] * m.m_t[11] + m_t[15] * m.m_t[15]
	);
}

template<typename Real>
inline Matrix4<Real> operator*(Real scalar, Matrix4<Real> const& m)
{
	return m * scalar;
}

}

#endif	//MATRIX4_H_
